#include "SimpliciTI/simplicitilib.h"
#include "driverlib/flash.h"

#include "wireless_task.h"
#include "helper.h"


static unsigned char DanceInstructionLength = sizeof(DanceInstruction);
static tBoolean isWirelessInitialized = false;
static tBoolean isWirelessLinked = false;
static linkID_t linkID = -1;
static int nextInstructionToGetIndex = 0;
static int nextInstructionInsertIndex = 0;
static DanceInstruction instructionBuffer[256];
static const int bufferMask = 0xFF;


char* statusToString(smplStatus_t status )
{
	switch(status)
	{
	case SMPL_SUCCESS:
		return "Success";
		break;

	case SMPL_TIMEOUT:
		return "Timeout";
		break;

	case SMPL_BAD_PARAM:
		return "Bad Parameter";
		break;

	case SMPL_NOMEM:
		return "No Memory";
		break;

	case SMPL_NO_FRAME:
		return "No Frame";
		break;

	case SMPL_NO_LINK:
		return "No Link";
		break;

	case SMPL_NO_JOIN:
		return "No Join";
		break;

	case SMPL_NO_CHANNEL:
		return "SNo Channel";
		break;

	case SMPL_NO_PEER_UNLINK:
		return "No Peer Unlink";
		break;

	case SMPL_TX_CCA_FAIL:
		return "TX cca fail";
		break;

	case SMPL_NO_PAYLOAD:
		return "No Payload";
		break;

	case SMPL_NO_AP_ADDRESS:
		return "No AP Address";
		break;

	case SMPL_NO_ACK:
		return "No Acknowledge";
		break;

	default:
		return "Unknown";
		break;
	}
	return "Unknown";
}

tBoolean setAddress()
{
	// Set the SimpliciTI device address as the least significant 4 digits of the
	// device Ethernet MAC address.  This ensures that the address is unique across
	// Stellaris devices.  If the MAC address has not been set, we return false to
	// indicate failure.
    unsigned long userRegister0;
    unsigned long userRegister1;
    addr_t address;

    // Make sure we are using 4 byte addressing.
    if (NET_ADDR_SIZE != 4)
    	displayMessage("ERROR", "4 byte addressing needed!");

    // Get the MAC address from the non-volatile user registers.
    FlashUserGet(&userRegister0, &userRegister1);

    // Has the MAC address been programmed?
    if((userRegister0 == 0xFFFFFFFF) || (userRegister1 == 0xFFFFFFFF))
    {
        // No - we don't have an address so return a failure.
    	displayMessage("ERROR", "MAC not set!");
        return false;
    }
    else
    {
        // The MAC address is stored with 3 bytes in each of the 2 flash user
        // registers.  Extract the least significant 4 MAC bytes for use as the
        // SimpliciTI device address.
        address.addr[0] = ((userRegister0 >>  0) & 0xFF);
        address.addr[1] = ((userRegister0 >>  8) & 0xFF);
        address.addr[2] = ((userRegister0 >> 16) & 0xFF);
        address.addr[3] = ((userRegister1 >> 16) & 0xFF);

        // SimpliciTI requires that the first byte of the device address is
        // never either 0x00 or 0xFF so we check for these cases and invert the
        // first bit if either is detected.  This does result in the
        // possibility of two devices having the same address but, for example
        // purposes, is likely to be fine.
        if((address.addr[0] == 0x00) || (address.addr[0] == 0xFF))
        {
            address.addr[0] ^= 0x80;
        }

        // Tell the SimpliciTI stack which device address we want to use.
        SMPL_Ioctl(IOCTL_OBJ_ADDR, IOCTL_ACT_SET, &address);
    }

    displayAddress(address.addr);
	waitMiliSeconds(2000);

    return true;
}

void setAddressHangOnError()
{
	if(setAddress() == false)
    {
        // No - display an error message.
        displayMessage("ERROR", "Address not set.");

        // Now hang.
        while(1) { }
    }
}

void setWirelessPower()
{
    // Set output power to +1.1dBm (868MHz) / +1.3dBm (915MHz)
	unsigned char ucPower = IOCTL_LEVEL_2;
    SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SETPWR, &ucPower);
}

void WirelessInit()
{
	BSP_Init();
	setAddressHangOnError();
	smplStatus_t result = SMPL_Init(0);
	setWirelessPower();

	if (result == SMPL_SUCCESS)
		isWirelessInitialized = true;
}

int wrapIndex(int index)
{
	return index & bufferMask;
}


void FollowerLink()
{
	smplStatus_t status = SMPL_NO_LINK;
	int attempt = 0;
	char* message;

	// Do nothing if wireless is not initialized
	if (isWirelessInitialized == false)
		return;

	while (status != SMPL_SUCCESS)
	{
		status = SMPL_Link(&linkID);
		attempt++;
		message = statusToString(status);
		displayMessagesAndNumber("Follower Link", message, attempt);
	}

	isWirelessLinked = true;
	displayMessageAndNumber("Follower Conn", (int) linkID);
}

tBoolean isBufferNotFull()
{
	if(wrapIndex(nextInstructionInsertIndex + 1) == nextInstructionToGetIndex)
		return false;
	else
		return true;
}

tBoolean wasInstructionReceived(DanceInstruction* newInstruction)
{
	unsigned char packetBuffer[MAX_APP_PAYLOAD];
	unsigned char packetLength;
	tBoolean success = false;

	// Do nothing if wireless is not linked
	if (isWirelessLinked == false)
		return false;

	//displayMessage("inst", "received");

	smplStatus_t packetResult = SMPL_Receive(linkID, packetBuffer, &packetLength);

	if(packetResult == SMPL_SUCCESS && packetLength == DanceInstructionLength)
	{
		newInstruction->leftTickCount = packetBuffer[0];
		newInstruction->leftWheelSpeed = packetBuffer[1];
		newInstruction->leftDirection = packetBuffer[2];
		newInstruction->rightTickCount = packetBuffer[3];
		newInstruction->rightWheelSpeed = packetBuffer[4];
		newInstruction->rightDirection = packetBuffer[5];
		success = true;
	}

	return success;
}

void WirelessTask(void *pvParam)
{
	DanceInstruction newInstruction;
	tBoolean instructionReceived = wasInstructionReceived(&newInstruction);
	tBoolean bufferNotFull = isBufferNotFull();

	if(instructionReceived == true && bufferNotFull == true)
	{
		//displayMessage("wireless", "send bitches");

		// Copy in the new instruction
		instructionBuffer[nextInstructionInsertIndex].leftDirection = newInstruction.leftDirection;
		instructionBuffer[nextInstructionInsertIndex].leftTickCount = newInstruction.leftTickCount;
		instructionBuffer[nextInstructionInsertIndex].leftWheelSpeed = newInstruction.leftWheelSpeed;
		instructionBuffer[nextInstructionInsertIndex].rightDirection = newInstruction.rightDirection;
		instructionBuffer[nextInstructionInsertIndex].rightTickCount = newInstruction.rightTickCount;
		instructionBuffer[nextInstructionInsertIndex].rightWheelSpeed = newInstruction.rightWheelSpeed;

		// Increment to the next insertion index
		nextInstructionInsertIndex = wrapIndex(nextInstructionInsertIndex + 1);

	}
	// else just drop the new instruction
}

DanceInstruction GetNextInstruction()
{
	DanceInstruction nextInstruction;

	//displayMessage("next", "instruction");

	// Copy the next instruction
	nextInstruction.leftDirection = instructionBuffer[nextInstructionToGetIndex].leftDirection;
	nextInstruction.leftTickCount = instructionBuffer[nextInstructionToGetIndex].leftTickCount;
	nextInstruction.leftWheelSpeed = instructionBuffer[nextInstructionToGetIndex].leftWheelSpeed;
	nextInstruction.rightDirection = instructionBuffer[nextInstructionToGetIndex].rightDirection;
	nextInstruction.rightTickCount = instructionBuffer[nextInstructionToGetIndex].rightTickCount;
	nextInstruction.rightWheelSpeed = instructionBuffer[nextInstructionToGetIndex].rightWheelSpeed;

	// Increment to the next instruction
	nextInstructionToGetIndex = wrapIndex(nextInstructionToGetIndex + 1);

	return nextInstruction;
}

tBoolean IsInstructionWaiting()
{
	if(nextInstructionToGetIndex == nextInstructionInsertIndex)
		return false;
	else
		return true;
}


